#include "utils.h"
#include "boost/algorithm/string/replace.hpp"

std::string Float1024ToString(float *feature)
{
  std::string ret("(");

  for ( int i = 0; i < 1023; i++) {
    ret.append(std::to_string(feature[i])).append(",");
  }
  ret.append(std::to_string(feature[1023])).append(")");

  return ret;
}

void StringToFloat1024v1(std::string str, float *feature)
{
  char* buffer = new char[str.length()+1];
  strcpy(buffer, str.c_str());

  int i=0, count=0;
  const char* delim = " ,*";
  char *p;

  p = strtok(buffer, delim);
  while(p){
    if (0 == i) {
      printf("%s\n", p);
    }
    else {
      float num = atof(p);
      if (count < 1024) feature[count] = num;
      count++;
    }
    p = strtok(NULL, delim);
    i++;
  }

  delete buffer;
}

void StringToFloat1024v2(std::string str, float *feature)
{
  char* buffer = new char[str.length()+1];
  strcpy(buffer, str.c_str());

  int count=0;
  const char* delim = " ,*()";
  char *p;

  p = strtok(buffer, delim);
  while(p){
    float num = atof(p);
    if (count < 1024) feature[count] = num;
    count++;
    p = strtok(NULL, delim);
  }

  delete buffer;
}

//Utils
boost::property_tree::ptree string2json(std::string json)
{
  boost::property_tree::ptree pt;
  std::stringstream sstream(json);
  boost::property_tree::json_parser::read_json(sstream, pt);
  return pt;
}

std::string json2string(boost::property_tree::ptree tree)
{
  std::stringstream ss;
  boost::property_tree::write_json(ss, tree, false);
  std::string json = ss.str();
  //replaceAll(json, "\\", "");
  boost::replace_all<std::string>(json, "\\", "");
  return json;
}

void replaceAll(std::string& base, std::string src, std::string dst)
{
  int pos = 0, srclen = src.size(), dstlen = dst.size();
  while ((pos = base.find(src, pos)) != std::string::npos)
  {
    base.replace(pos, srclen, dst);
    pos += dstlen;
  }
}


//Base64-Mat
std::string base64Decode(const char* Data, int DataByte)
{
  const char DecodeTable[] =
  {
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    62, // '+'
    0, 0, 0,
    63, // '/'
    52, 53, 54, 55, 56, 57, 58, 59, 60, 61, // '0'-'9'
    0, 0, 0, 0, 0, 0, 0,
    0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12,
    13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, // 'A'-'Z'
    0, 0, 0, 0, 0, 0,
    26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38,
    39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, // 'a'-'z'
  };

  std::string strDecode;
  int nValue;
  int i = 0;

  while (i < DataByte) {
    if (*Data != '\r' && *Data != '\n') {
      nValue = DecodeTable[*Data++] << 18;
      nValue += DecodeTable[*Data++] << 12;
      strDecode += (nValue & 0x00FF0000) >> 16;
      if (*Data != '=') {
        nValue += DecodeTable[*Data++] << 6;
        strDecode += (nValue & 0x0000FF00) >> 8;
        if (*Data != '=') {
          nValue += DecodeTable[*Data++];
          strDecode += nValue & 0x000000FF;
        }
      }
      i += 4;
    }
    else {
      Data++;
      i++;
    }
  }
  return strDecode;
}

std::string base64Encode(const unsigned char* Data, int DataByte)
{
  const char EncodeTable[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";

  std::string strEncode;
  unsigned char Tmp[4] = { 0 };
  int LineLength = 0;

  for (int i = 0; i < (int)(DataByte / 3); i++) {
    Tmp[1] = *Data++;
    Tmp[2] = *Data++;
    Tmp[3] = *Data++;
    strEncode += EncodeTable[Tmp[1] >> 2];
    strEncode += EncodeTable[((Tmp[1] << 4) | (Tmp[2] >> 4)) & 0x3F];
    strEncode += EncodeTable[((Tmp[2] << 2) | (Tmp[3] >> 6)) & 0x3F];
    strEncode += EncodeTable[Tmp[3] & 0x3F];
    //if (LineLength += 4, LineLength == 76) { strEncode += "\r\n"; LineLength = 0; }
    if (LineLength += 4, LineLength == 76) { LineLength = 0; }
  }

  int Mod = DataByte % 3;
  if (Mod == 1) {
    Tmp[1] = *Data++;
    strEncode += EncodeTable[(Tmp[1] & 0xFC) >> 2];
    strEncode += EncodeTable[((Tmp[1] & 0x03) << 4)];
    strEncode += "==";
  }
  else if (Mod == 2) {
    Tmp[1] = *Data++;
    Tmp[2] = *Data++;
    strEncode += EncodeTable[(Tmp[1] & 0xFC) >> 2];
    strEncode += EncodeTable[((Tmp[1] & 0x03) << 4) | ((Tmp[2] & 0xF0) >> 4)];
    strEncode += EncodeTable[((Tmp[2] & 0x0F) << 2)];
    strEncode += "=";
  }
  return strEncode;
}

std::string Mat2Base64(const cv::Mat &img, std::string imgType)
{
  std::string img_data;
  std::vector<uchar> vecImg;
  std::vector<int> vecCompression_params;
  vecCompression_params.push_back(CV_IMWRITE_JPEG_QUALITY);
  vecCompression_params.push_back(90);
  imgType = "." + imgType;
  cv::imencode(imgType, img, vecImg, vecCompression_params);
  img_data = base64Encode(vecImg.data(), vecImg.size());
  return img_data;
}

cv::Mat Base2Mat(std::string &base64_data)
{
  cv::Mat img;
  std::string s_mat;
  s_mat = base64Decode(base64_data.data(), base64_data.size());
  std::vector<char> base64_img(s_mat.begin(), s_mat.end());
  img = cv::imdecode(base64_img, CV_LOAD_IMAGE_COLOR);
  return img;
}


//OpenCV Extends
void getRTMatrix_l(const cv::Point2f* a, const cv::Point2f* b,int count, cv::Mat& M, bool fullAffine)
{
  CV_Assert(M.isContinuous());
  if (fullAffine) {
    double sa[6][6] = { { 0. } }, sb[6] = { 0. };
    cv::Mat A(6, 6, CV_64F, &sa[0][0]), B(6, 1, CV_64F, sb);
    cv::Mat MM = M.reshape(1, 6);

    for (int i = 0; i < count; i++)
    {
      sa[0][0] += a[i].x*a[i].x;
      sa[0][1] += a[i].y*a[i].x;
      sa[0][2] += a[i].x;

      sa[1][1] += a[i].y*a[i].y;
      sa[1][2] += a[i].y;

      sb[0] += a[i].x*b[i].x;
      sb[1] += a[i].y*b[i].x;
      sb[2] += b[i].x;
      sb[3] += a[i].x*b[i].y;
      sb[4] += a[i].y*b[i].y;
      sb[5] += b[i].y;
    }

    sa[3][4] = sa[4][3] = sa[1][0] = sa[0][1];
    sa[3][5] = sa[5][3] = sa[2][0] = sa[0][2];
    sa[4][5] = sa[5][4] = sa[2][1] = sa[1][2];

    sa[3][3] = sa[0][0];
    sa[4][4] = sa[1][1];
    sa[5][5] = sa[2][2] = count;

    cv::solve(A, B, MM, cv::DECOMP_EIG);
  }
  else {
    double sa[4][4] = { { 0. } }, sb[4] = { 0. }, m[4] = { 0 };
    cv::Mat A(4, 4, CV_64F, sa), B(4, 1, CV_64F, sb);
    cv::Mat MM(4, 1, CV_64F, m);

    for (int i = 0; i < count; i++)
    {
      sa[0][0] += a[i].x*a[i].x + a[i].y*a[i].y;
      sa[0][2] += a[i].x;
      sa[0][3] += a[i].y;

      sb[0] += a[i].x*b[i].x + a[i].y*b[i].y;
      sb[1] += a[i].x*b[i].y - a[i].y*b[i].x;
      sb[2] += b[i].x;
      sb[3] += b[i].y;
    }

    sa[1][1] = sa[0][0];
    sa[2][1] = sa[1][2] = -sa[0][3];
    sa[3][1] = sa[1][3] = sa[2][0] = sa[0][2];
    sa[2][2] = sa[3][3] = count;
    sa[3][0] = sa[0][3];

    cv::solve(A, B, MM, cv::DECOMP_EIG);

    double* om = M.ptr<double>();
    om[0] = om[4] = m[0];
    om[1] = -m[1];
    om[3] = m[1];
    om[2] = m[2];
    om[5] = m[3];
  }
}
cv::Mat estimateRigidTransform_l(cv::InputArray src1, cv::InputArray src2, bool fullAffine)
{
  cv::Mat M(2, 3, CV_64F), A = src1.getMat(), B = src2.getMat();
  int good_count = 5;
  std::vector<cv::Point2f> pA, pB;

  A.reshape(2, good_count).convertTo(pA, CV_32F);
  B.reshape(2, good_count).convertTo(pB, CV_32F);

  getRTMatrix_l(&pA[0], &pB[0], good_count, M, fullAffine);
  return M;
}

float feature_cmp1024(float *feats_src, float *feats_dst, cv::Mat feature_meanval)
{
  cv::Mat featrue1(1, 1024, CV_32FC1, feats_src);
  cv::Mat featrue2(1, 1024, CV_32FC1, feats_dst);
  cv::Mat featrue1_Sub(1, 1024, CV_32FC1);
  cv::Mat featrue2_Sub(1, 1024, CV_32FC1);

  if (feature_meanval.empty()) {
    std::cerr << "[Error]: No Found Feature Mean Value Map!" << std::endl;
    return 0;
  }

  featrue1_Sub = featrue1 - feature_meanval;
  featrue2_Sub = featrue2 - feature_meanval;

  double modeProduct = cv::norm(featrue1_Sub, 4)*cv::norm(featrue2_Sub, 4);
  double similarity = featrue1_Sub.dot(featrue2_Sub) / modeProduct;
  float resultScore = (similarity + 1.0) / 2.0;
  return resultScore;
}


//IniParser
IniParser::IniParser(char* filename)
{
  this->filename = filename;
  boost::property_tree::ini_parser::read_ini(filename, this->lvptProperties);
}

IniParser::~IniParser()
{
}

int IniParser::GetInt(char* key, char* session)
{
  auto lvbtItems = lvptProperties.get_child(session);
  int lvnInt = 0;
  try {
    lvnInt = lvbtItems.get<int>(key);
  }
  catch (std::exception& e) {
    std::cerr << e.what() << std::endl;
  }
  return lvnInt;
}

void IniParser::PutInt(char* key, int val, char* session)
{
  std::string ckey = std::string(session) + "." + std::string(key);
  this->lvptProperties.put<int>(ckey, val);
  boost::property_tree::ini_parser::write_ini(this->filename, this->lvptProperties);
}

std::string IniParser::GetString(char* key, char* session)
{
  auto lvbtItems = lvptProperties.get_child(session);
  std::string lvnString;
  try {
    lvnString = lvbtItems.get<std::string>(key);
  }
  catch (std::exception& e) {
    std::cerr << e.what() << std::endl;
  }
  return lvnString;
}

void IniParser::PutString(char* key, const char* val, char* session)
{
  std::string ckey = std::string(session) + "." + std::string(key);
  this->lvptProperties.put<std::string>(ckey, val);
  boost::property_tree::ini_parser::write_ini(this->filename, this->lvptProperties);
}

float IniParser::GetFloat(char* key, char* session)
{
  auto lvbtItems = lvptProperties.get_child(session);
  float lvnFloat = 0;
  try {
    lvnFloat = lvbtItems.get<float>(key);
  }
  catch (std::exception& e) {
    std::cerr << e.what() << std::endl;
  }
  return lvnFloat;
}

void IniParser::PutFloat(char* key, float val, char* session)
{
  std::string ckey = std::string(session) + "." + std::string(key);
  this->lvptProperties.put<float>(ckey, val);
  boost::property_tree::ini_parser::write_ini(this->filename, this->lvptProperties);
}

